/*******************************************************************************
* Copyright 2017 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

/* Author: Jin Jian Liu */

#if defined(STM32F103xB)

#include "main.h"
#include "string.h"
#include "buffer.h"

#include "port_handler_stm32.h"


#define LATENCY_TIMER     4  // msec (USB latency timer)

using namespace dynamixel;

PortHandlerSTM32::PortHandlerSTM32(const char *port_name)
  : baudrate_(DEFAULT_BAUDRATE_),
    packet_start_time_(0.0),
    packet_timeout_(0.0),
    tx_time_per_byte(0.0)
{
  is_using_ = false;
  setPortName(port_name);

  /* 指示灯初始化 */
  
	/* 初始关闭指示灯 */
  setPowerOff();
}

bool PortHandlerSTM32::openPort()
{
  setPowerOn();

  return setBaudRate(baudrate_);
}

void PortHandlerSTM32::closePort()
{
  setPowerOff();
}

void PortHandlerSTM32::clearPort()
{
  /* 清空串口数据 */
	BufferClear();
}

void PortHandlerSTM32::setPortName(const char *port_name)
{
  strcpy(port_name_, port_name);
}

char *PortHandlerSTM32::getPortName()
{
  return port_name_;
}

bool PortHandlerSTM32::setBaudRate(const int baudrate)
{
  baudrate_ = checkBaudrateAvailable(baudrate);

  if (baudrate_ == -1)
    return false;

  setupPort(baudrate_);

  return true;
}

int PortHandlerSTM32::getBaudRate()
{
  return baudrate_;
}

int PortHandlerSTM32::getBytesAvailable()
{
  int bytes_available;

  /* 判断串口数据缓冲区是否存在数据，返回串口缓冲区中当前剩余的字符个数 */
	bytes_available = BufferAvailable();
  return bytes_available;
}

int PortHandlerSTM32::readPort(uint8_t *packet, int length)
{
  int rx_length;
  rx_length = getBytesAvailable();


  if (rx_length > length)
    rx_length = length;

  
	/* 读取串口缓冲区的数据 */
	BufferRead(packet, rx_length);

  return rx_length;
}

int PortHandlerSTM32::writePort(uint8_t *packet, int length)
{
  int length_written;
	length_written = length;
  /* 发送串口数据并返回写入的字节数 */
	BufferWrite(packet, length_written);
  
  return length_written;
}

void PortHandlerSTM32::setPacketTimeout(uint16_t packet_length)
{
  packet_start_time_  = getCurrentTime();
  packet_timeout_     = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0;
}

void PortHandlerSTM32::setPacketTimeout(double msec)
{
  packet_start_time_  = getCurrentTime();
  packet_timeout_     = msec;
}

bool PortHandlerSTM32::isPacketTimeout()
{
  if (getTimeSinceStart() > packet_timeout_)
  {
    packet_timeout_ = 0;
    return true;
  }

  return false;
}

double PortHandlerSTM32::getCurrentTime()
{
  /* 获得当前的timebase */
  return (double)HAL_GetTick();
}

double PortHandlerSTM32::getTimeSinceStart()
{
  double elapsed_time;

  elapsed_time = getCurrentTime() - packet_start_time_;
  if (elapsed_time < 0.0)
    packet_start_time_ = getCurrentTime();

  return elapsed_time;
}

bool PortHandlerSTM32::setupPort(int baudrate)
{
  
  /* 串口初始化 */

  tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0;
  return true;
}


int PortHandlerSTM32::checkBaudrateAvailable(int baudrate)
{
  switch(baudrate)
  {
    case 9600:
      return 9600;
    case 57600:
      return 57600;
    case 115200:
      return 115200;
    case 1000000:
      return 1000000;
    case 2000000:
      return 2000000;
    case 3000000:
      return 3000000;
    case 4000000:
      return 4000000;
    case 4500000:
      return 4500000;
    default:
      return -1;
  }
}

void PortHandlerSTM32::setPowerOn()
{
  /* 打开指示灯 */
	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);
}

void PortHandlerSTM32::setPowerOff()
{
  /* 关闭指示灯 */
	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);
}

void PortHandlerSTM32::setTxEnable()
{
  /* 打开串口发送 */
}

void PortHandlerSTM32::setTxDisable()
{
  /* 关闭串口发送 */
}

#endif
